/*
 * VanishPoint.cpp
 *
 *  Created on: 18/07/2010
 *      Author: alejo
 */
#include "VanishPoint.h"
#include <iostream>
#include <stdio.h>
#include <opencv/cv.h>
//#include <opencv/highgui.h>

/**
 * Construct Vanish Point with default parameters
 */
VanishPoint::VanishPoint()
{

	//set default canny parameters
	this->lowThresh = 50;
	this->highThresh = 100;
	this->gWindow = 3;

	//set line detection parameters
	this->houghMethod = CV_HOUGH_STANDARD;
	this->minLineLength = 50;
	this->rhoRes = 1;
	this->angleRes = CV_PI/180;
	this->param1 = 0;
	this->param2 = 0;
	this->maxLines = 50;

	//set line clasification parameters
	this->minAngleV = CV_PI/18;
	this->maxAngleV = CV_PI - CV_PI/18;
	this->minAngleH = CV_PI/2 +CV_PI/18;
	this->maxAngleH = CV_PI/2 - CV_PI/18;
	this->minDifAngle = CV_PI/10;


	this->fidelidad = 0;
	//memory storage used in the Hough transform
	storage = cvCreateMemStorage(0);

	//array of detected lines
	lines = new Line[maxLines];
	//crossing of lines are assumed to be lower than number of lines^2
	crossingPoints = new CvPoint[maxLines*maxLines];

	//lines detected by Hough transform
	hLines = 0;
}

void VanishPoint::initialize(int imgWidth, int imgHeight)
{
	//set img parameters
	this->imgHeight = imgHeight;
	this->imgWidth = imgWidth;

	canny  = cvCreateImage(cvSize(imgWidth,imgHeight),IPL_DEPTH_8U,1);	
}

VanishPoint::~VanishPoint()
{
	delete[] lines;
	delete[] crossingPoints;

}

/**
 * Sets the parameters for the line classification stage.
 */
void VanishPoint::setClasiffParams(float minAngleV, float maxAngleV, float minAngleH, float maxAngleH, float minDifAngle)
{
	//set line clasification parameters
	this->minAngleV = minAngleV;
	this->maxAngleV = maxAngleV;
	this->minAngleH = minAngleH;
	this->maxAngleH = maxAngleH;
	this->minDifAngle = minDifAngle;
}

/**
 * Sets the parameters for the Canny  (Border detection) stage.
 */
void VanishPoint::setCannyParams(unsigned int lowThresh, unsigned int highThresh, unsigned int gWindow)
{
	//set canny parameters
	this->lowThresh = lowThresh;
	this->highThresh = highThresh;
	this->gWindow = gWindow;
}

/**
 * Sets the parameters of the Hough (line detection) stage.
 */
void VanishPoint::setHoughParams(int houghMethod, int minLineLength, double rhoRes, double angleRes, double param1, double param2, int maxLines)
{
	this->houghMethod = houghMethod;
	this->minLineLength = minLineLength;
	this->rhoRes = rhoRes;
	this->angleRes = angleRes;
	this->param1 = param1;
	this->param2 = param2;
	this->maxLines = maxLines;

	//recalculate line and crossing point arrays
	delete[] lines;
	delete[] crossingPoints;

	//array of detected lines
	lines = new Line[maxLines];
	
	//crossing of lines are assumed to be lower than number of lines^2
	crossingPoints = new CvPoint[maxLines*maxLines];

}


/**
 * Returns a copy of the Canny image obtained by the process function
 */
IplImage* VanishPoint::getCannyImg()
{
	IplImage* img = cvCreateImage(cvSize(imgWidth,imgHeight),IPL_DEPTH_8U,1);
	cvCopy(this->canny,img);
	return img;
}

IplImage* VanishPoint::doCanny(
    IplImage* in,
    double     lowThresh,
    double     highThresh,
    double     aperture
) {
    if(in->nChannels != 1)
    {
    	printf("Solo imagenes en escala de gris\n");
        return(0); //Canny only handles gray scale images

    }

    cvCanny( in, canny, lowThresh, highThresh, aperture );
    return( canny);
}

void VanishPoint::process(IplImage* img, bool fidEnable)
{
	//images used in the process
	IplImage* gray = cvCreateImage(cvSize(imgWidth,imgHeight),IPL_DEPTH_8U,1);

	cvCvtColor(img, gray, CV_RGB2GRAY);

	//__________________________________________________
	// CANNY STAGE
	//__________________________________________________
	canny = doCanny(gray, this->lowThresh, this->highThresh, this->gWindow);

	//__________________________________________________
	// LINE DETECTION STAGE
	//__________________________________________________

	hLines = cvHoughLines2( canny, storage, this->houghMethod, this->rhoRes, this->angleRes, this->minLineLength, this->param1, this->param2 );


	validLines = 0;

	//FIXME: Esto no se si deberia
	//delete[] lines;
	//lines = new Line[maxLines];

	crossCount = 0;

	//__________________________________________________
	// LINE CLASIFFICATION STAGE
	//__________________________________________________

	//Filtrar lineas por angulos. Las lineas verticales y horizontales son
	//descartadas.
	int lineCount = MIN(hLines->total,(int)maxLines);

	for(int i = 0; i < lineCount; i++ )
	{
		float* line = (float*)cvGetSeqElem(hLines,i);
		float rho = line[0];
		float theta = line[1];

		//points that describe the line
		CvPoint pt1, pt2;

		if((theta > this->minAngleV && theta < this->maxAngleH) || (theta > this->minAngleH && theta < this->maxAngleV))

		{

			double a = cos(theta), b = sin(theta);
			double x0 = a*rho, y0 = b*rho;

			pt1.x = cvRound(x0 + 1000*(-b));
			pt1.y = cvRound(y0 + 1000*(a));
			pt2.x = cvRound(x0 - 1000*(-b));
			pt2.y = cvRound(y0 - 1000*(a));

			//add points to the validLines array
			lines[validLines].p0 = pt1;
			lines[validLines++].p1 = pt2;

			//identify line crossings
			if(validLines>1)
			{
				for(int j = 0; j<validLines-1;j++)
				{
					int x0 = lines[j].p0.x;
					int x1 = lines[j].p1.x;
					int x2 = lines[validLines-1].p0.x;
					int x3 = lines[validLines-1].p1.x;

					int y0 = lines[j].p0.y;
					int y1 = lines[j].p1.y;
					int y2 = lines[validLines-1].p0.y;
					int y3 = lines[validLines-1].p1.y;

					int cociente = (y1-y0)*(x3-x2)-(y3-y2)*(x1-x0);

					float ang1 = atan2(y1-y0,x1-x0);
					float ang2 = atan2(y3-y2,x3-x2);


					//cruces de lineas casi paralelas no son tomados en cuenta!
					float diffAngle = fabs(ang1 - ang2);

					//las lineas son paralelas?
					if(cociente!= 0 && diffAngle > (this->minDifAngle))
					{
						int x,y;
						float m,b;

						int numerador = (x1-x0)*(y2*x3-y3*x2)+(y1*x0-y0*x1)*(x3-x2);

						m = (float)(y1-y0)/(float)(x1-x0);
						b = y0 - m*x0;

						x = numerador / cociente;
						y = m*x + b;

						CvPoint cruc = cvPoint(x,y);

						//first check if crossing are located within the image borders
						if(cruc.x>0 && cruc.x<imgWidth && cruc.y>0 && cruc.y<imgHeight)
						{
							//save intersections
							crossingPoints[crossCount++] = cruc;
						}

					}

				}
			}


		}
	}


	//__________________________________________________
	// COST FUNCTION FOR VANISHING POINT ESTIMATION
	//__________________________________________________

	//definir para el valor inicial de la minima distancia el valor mas grande que se pueda presentar
	double minDist = sqrt(this->imgWidth*this->imgWidth+this->imgHeight*this->imgHeight)*crossCount;
	CvPoint vpoint;

	//TODO: utilizar los for anteriores para calcular el VP
	//iterar por todos los puntos de cruce
	for(int w = 0; w <crossCount; w++)
	{
		double dist = 0;
		//calculo de la distancia con todos los puntos de cruce
		for(int z = 0; z<crossCount; z++)
		{
			dist += sqrt((crossingPoints[w].x-crossingPoints[z].x)*(crossingPoints[w].x-crossingPoints[z].x) + (crossingPoints[w].y-crossingPoints[z].y)*(crossingPoints[w].y-crossingPoints[z].y));
		}

		//actualizar la minima distancia, si la nueva es menor
		if(dist<minDist)
		{
			minDist = dist;
			vpoint.x = crossingPoints[w].x;
			vpoint.y = crossingPoints[w].y;
		}

	}

	//calculate fidelity of vanishing point only if desired
	if(fidEnable)
	{
		//fidelidad sera un numero utilizado para saber que tan fiable es el punto de fuga encontrado
		fidelidad = 0;
		//pesos para la funcion de costo
		float k1 = 0.4f;
		float k2 = 0.6f;

		int ptosVecindad = 0;

		//TODO: Otro for para la fidelidad????
		for(int i = 0; i<crossCount; i++)
		{
			float distX = vpoint.x-crossingPoints[i].x;
			float distY = vpoint.y-crossingPoints[i].y;

			float distancia = sqrt(distX*distX + distY*distY);
			if(distancia<30)
				ptosVecindad++;
		}

		fidelidad = crossCount*k1 + ptosVecindad*k2;

		fidelidad = (((float)fidelidad/(float)crossCount)*100);
	}

	//if no crossing points were found, there is no vanishing point.
	//then localize the vanishing point outside the image (-1,-1)
	if(crossCount>0)
	{
		this->vp.x = vpoint.x;
		this->vp.y = vpoint.y;
	}
	else
	{
		this->vp.x = -1;
		this->vp.y = -1;

	}

	//destroy all variables
	cvReleaseImage(&gray);

}

/**
 * Get the detected lines by Hough transform
 */
Line* VanishPoint::getDetectedLines(void)
{
	return this->lines;
}

/**
 * Gets the number of detected lines.
 */
int VanishPoint::getLinesCount(void)
{
	return this->validLines;

}
/**
 * The detected vanishing point
 */
CvPoint VanishPoint::getVanishPoint(void)
{
	return this->vp;
}

/**
 * The crossing points between lines detected in the image.
 */
CvPoint* VanishPoint::getCrossingPoints(void)
{
	return this->crossingPoints;
}

/**
 * The number of crossing points in the image.
 */
int VanishPoint::getCrossPointCount(void)
{
	return this->crossCount;
}
